#include <buola/cv/ccloud.h>
#include <buola/scene.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/app/ceventloop.h>
#include <buola/image/io.h>
#include <buola/image/cgc.h>
#include <buola/image/algorithm/structure.h>
#include <buola/app/ccmdline.h>
#include <buola/app/capp.h>
#include <buola/scene/cscene.h>
#include <buola/scene/cimagerenderer.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/gui/ctoolkit.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/algorithm/range.h>

using namespace buola;

static const mat::CVec3d lCameraAt{0,1,-0.98};
static const mat::CVec3d lCameraPos{0,0,0};
static const mat::CVec3d lCameraUp{0,0,1};

void OnTimer(scene::PPointCloud pSceneCloud,scene::CSceneView &pView)
{
    static int lIndex=0;
    start_timer();
    cvi::CCloud_rgb8 lCloud=cvi::load_cloud(io::file("frame"+to_string(lIndex++)+".pcd"));
    end_timer();
    start_timer();
    lCloud=cvi::transform_lookat(lCloud,mat::CVec3f(lCameraAt),mat::CVec3f(lCameraPos),mat::CVec3f(lCameraUp));
    end_timer();
//     start_timer();
//     lCloud=cvi::filter(lCloud,[](const cvi::CCloud_rgb8::TPoint &pP)
//         {
// //            if(pP.mPixel[0]>100||pP.mPixel[1]>100||pP.mPixel[2]<10) return false;
//             if(pP.mPixel[2]<=pP.mPixel[1]||pP.mPixel[2]<=pP.mPixel[0]) return false;
//              if(pP.mPoint.x()<-0.45f) return false;
//              if(pP.mPoint.x()>0.3f) return false;
//              if(pP.mPoint.y()<0.6f) return false;
//              if(pP.mPoint.y()>2.0f) return false;
// //             if(pP.mPoint.z()>-0.7) return false;
//             return true;
//         });
//     end_timer();
    
    typedef typename cvi::CCloud_rgb8::TPoint TPoint;
    
    start_timer();
    auto lMedian=median(lCloud,[](const TPoint &p1,const TPoint &p2){return p1.mPoint.z()<p2.mPoint.z();});
    end_timer("median");
    
    msg_info() << "median=" << lMedian.mPoint.T() << "\n"; 
    start_timer();
    auto lMin=min_value(lCloud,[](const TPoint &p1,const TPoint &p2){return p1.mPoint.z()<p2.mPoint.z();});
    end_timer("min");
    msg_info() << "min=" << lMin.mPoint.T() << "\n"; 
    auto lMax=max_value(lCloud,[](const TPoint &p1,const TPoint &p2){return p1.mPoint.z()<p2.mPoint.z();});
    msg_info() << "max=" << lMax.mPoint.T() << "\n"; 
    msg_info() << "diff:" << lMax.mPoint.z()-lMin.mPoint.z() << "\n";
    
    std::vector<CPoint_d> lPoints;
    for(int i=0;i<lCloud.size();i++)
    {
        lPoints.emplace_back(lCloud[i].mPoint.x(),lCloud[i].mPoint.y());
    }
    start_timer();
    auto lCorners=img::min_area_rect(lPoints);
    end_timer("corners");
    msg_info() << "corners\n";
    for(const auto &c : lCorners)
    {
        mat::CVec3f lCorner(c.x,c.y,lMedian.mPoint.z());
        auto lMin=min_element(lCloud,[&lCorner](const TPoint &p1,const TPoint &p2)
                                      {return distance(p1.mPoint,lCorner)<distance(p2.mPoint,lCorner);});
        lMin->mPixel=img::pixel(255,0,0);
    }
    
    
    {
        msg_info() << "got one!\n";
        lock_guard<recursive_mutex> lLock(get_main_event_loop().GetMutex());
        pSceneCloud->SetCloud(std::move(lCloud));
        pView.Refresh();
    }
}

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    try
    {
        CApp lApp;
        
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();

        //create the scene
        scene::PScene lScene=new scene::CScene;
        
        scene::PCube lCube=new scene::CCube;
        scene::PRTTransform lTransform=new scene::CRTTransform(mat::CQuat_d(),{0,0,5});
        lCube->AttachTo(lTransform);
        lScene->World()->AddChild(lTransform);

        scene::PPointCloud lCloud=new scene::CPointCloud();
        lScene->World()->AddObject(lCloud);

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1,50.0);
        lCamera->LookAt(lCameraAt,lCameraPos,lCameraUp);

        scene::PSceneView lView=new scene::CSceneView;
        lView->SetCamera(lCamera);
        lView->AddScene(lScene);
        lView->AddController(new scene::CCameraController(lCamera));
        lView->Map(CSize_d(800,600));

        CTimer lTimer(500_ms,true);
        lTimer.sTimer.Connect([&]{   OnTimer(lCloud,*lView);  });
        
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
